perm filename ARMSOL[SYS,HE]3 blob
sn#101765 filedate 1974-05-10 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00015 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 TITLE ARMSOL
C00003 00003 THESE ROUTINES TOGETHER WITH SAITRG[SYS,HE] OCCUPY 1243 OCTAL WORDS
C00004 00004 BEGIN CROSS
C00005 00005 BEGIN SECOND
C00006 00006 BEGIN ANGLE
C00008 00007 BEGIN SOLVE
C00012 00008 WE NOW NEED SOME SINES AND COSINES
C00014 00009 NOW JOINT 4
C00017 00010 DEGEN: SETZM J+4 JOINT 5 IS ZERO
C00018 00011 PRINC: 0
C00019 00012 IFE WAVE,<
C00022 00013 MUL: MOVEI J,2
C00023 00014 TH: BLOCK 6
C00024 00015 BEGIN TRANSMULT
C00025 ENDMK
C⊗;
TITLE ARMSOL
↓WAVE←←1 ;just assemble the solution for wave
↓BLUE←←0 ;assemble for blue arm
IFN BLUE,<
↓LH←←1 ;left hand arm (blue)
↓RH←←0 ;rignt hand arm (yellow)
>
IFE BLUE,<
↓LH←←0 ;left hand arm (blue)
↓RH←←1 ;rignt hand arm (yellow)
>
;THESE ROUTINES TOGETHER WITH SAITRG[SYS,HE] OCCUPY 1243 OCTAL WORDS
IFN WAVE,<
INTERNAL ARM.SOLVE
>
IFE WAVE,<
INTERNAL ANGLE,SOLVE,UPDATE,TIMES
>
INTERNAL PSOLVE
EXTERNAL ACOS,SQRT,ATAN2,ASIN,ACOS,SIND,COSD
↓AC←1
↓P←17
IFE BLUE,<
S2V←←6.05
S6V←←9.38
>
IFN BLUE,<
S2V←←-6.05
S6V←←9.425
>
S22V←←36.6025
↓RAD: 206712273406
IFN BLUE,<
↓SHOLDER:20.035
51.305
20.24
>
IFE BLUE,<
↓SHOLDER:20.035
10.305
16.24
>
BEGIN CROSS
;CROSS(R,P,A) RETURNS THE CROSS PRODUCT IN R
;TIME APPROX 180 MICRO SECONDS
TAC1←←2
PV1←←3
PV2←←4
PV3←←5
AV1←←6
AV2←←7
AV3←←10
O←←11
↑CROSS:
HRL AC,-2(P) ;MOVE PV INTO AC'S
HRRI AC,PV1
BLT AC,PV3
HRL AC,-1(P) ;MOVE AV INTO AC'S
HRRI AC,AV1
BLT AC,AV3
MOVE O,-3(P)
MOVE AC,PV2
FMPR AC,AV3
MOVE TAC1,PV3
FMPR TAC1,AV2
FSBR AC,TAC1
MOVEM AC,(O)
FMPR PV3,AV1
FMPR AV3,PV1
FSBR PV3,AV3
MOVEM PV3,1(O)
FMPR PV1,AV2
FMPR AV1,PV2
FSBR PV1,AV1
MOVEM PV1,2(O)
SUB P,[4(4)]
JRST @4(P)
BEND
BEGIN SECOND
;SECOND(A) RETURNS THE MAGNITUDE SQUARED OF THE VECTOR
;TIME APPROX 90 MICRO SEC
A←←4
↑SECOND:
HRL A,-1(P)
HRRI A,AC
BLT A,AC+2
FMPR AC,AC
FMPR AC+1,AC+1
FMPR AC+2,AC+2
FADR AC,AC+1
FADR AC,AC+2
SUB P,[2(2)]
JRST @2(P)
BEND
BEGIN ANGLE
;ANGLE(P,A,O) RETURNS THE ANGLE BETWEEN P AND A IN DEGREES ABOUT O
;A AND P MUST BE UNIT VECTORS. I.E. DIRECTION COSINES
;COMPUTES THE ANGLE=ACOS(PV1*AV1 + PV2*AV2 + PV3*AV3)
;AND THEN THE DETERMINANT
; | O1 O2 O3|
; |PV1 PV2 PV3|
; |AV1 AV2 AV3|
;AND IF POSITIVE MULTIPLIES THE ANGLE BY -1;
;TIME APPROX 550 MICRO SECONDS
TAC1←←2
PV1←←3
PV2←←4
PV3←←5
AV1←←6
AV2←←7
AV3←←10
O←←11
I←←13 ;12 IS A NO NO
↑ANGLE:
HRL AC,-3(P) ;MOVE PV INTO AC'S
HRRI AC,PV1
BLT AC,PV3
HRL AC,-2(P) ;MOVE AV INTO AC'S
HRRI AC,AV1
BLT AC,AV3
MOVEI I,2 ;COMPUTE DOT PRODUCT
MOVE AC,PV1
FMPR AC,AV1
L1: MOVE TAC1,PV1(I)
FMPR TAC1,AV1(I)
FADR AC,TAC1
SOJG I,L1
MOVEM AC,DOT
MOVE O,-1(P) ;COMPUTE DETERMINANT
MOVE AC,PV2
FMPR AC,AV3
MOVE TAC1,PV3
FMPR TAC1,AV2
FSBR AC,TAC1
FMPR AC,(O)
FMPR PV3,AV1
FMPR AV3,PV1
FSBR PV3,AV3
FMPR PV3,1(O)
FADR AC,PV3
FMPR PV1,AV2
FMPR AV1,PV2
FSBR PV1,AV1
FMPR PV1,2(O)
FADR AC,PV1
MOVEM AC,DET
PUSH P,DOT
PUSHJ P,ACOS
FMPR AC,RAD
SKIPL DET ;NEGATE ANGLE IF DET NEGATIVE
MOVN AC,AC
SUB P,[4(4)]
JRST @4(P)
DOT: 0
DET: 0
BEND
BEGIN SOLVE
;SOLVE(T,J) GIVEN THE TRANSFORM T, RETURNS THE SOLUTION IN "J"
;PSOLVE(T,J) GIVEN THE TRANSFORM T, RETURNS THE PRINCIPAL SOLUTION IN "J"
;RETURNS FALSE IF THE SOLUTION IS CORRECT
;TIME APPROX 4 M. SEC
TAC←←2
I←←3
↑PSOLVE:SETOM PRINC
SKIPA
IFE WAVE,<
↑SOLVE:
>
IFN WAVE,<
↑ARM.SOLVE:
>
SETZM PRINC ;DO NOT USE PRINCIPAL SOLUTION
SETZM FLAG ;SET RETURN FLAG TRUE
HRL AC,-1(P) ;INITIALIZE J
HRRI AC,J
BLT AC,J+5
;COMPUTE W=P-SHOLDER-A*S6
HRRZ AC,-2(P) ;MOVE P INTO MEMORY
MOVE TAC,3(AC)
MOVEM TAC,PV1
MOVE TAC,7(AC)
MOVEM TAC,PV1+1
MOVE TAC,13(AC)
MOVEM TAC,PV1+2
MOVE TAC,2(AC) ;MOVE A INTO MEMORY
MOVEM TAC,AV1
MOVE TAC,6(AC)
MOVEM TAC,AV1+1
MOVE TAC,12(AC)
MOVEM TAC,AV1+2
MOVE TAC,1(AC) ;MOVE O INTO MEMORY
MOVEM TAC,OV1
MOVE TAC,5(AC)
MOVEM TAC,OV1+1
MOVE TAC,11(AC)
MOVEM TAC,OV1+2
;J2=DOT(W,W)-S2↑2
MOVN TAC,S22 ;INITIALIZE TAC TO -S22
MOVEI I,2 ;COMPUTE THE DOT PRODUCT
L1: MOVE AC,AV1(I)
FMPR AC,S6
FADR AC,SHOLDER(I)
FSBR AC,PV1(I)
MOVEM AC,W(I) ;-W(I)
FMPR AC,AC
MOVEM AC,W2(I) ;W(I)↑2
FADR TAC,AC
SOJGE I,L1
JUMPLE TAC,[SETZ AC,
JRST NSR]
PUSH P,TAC
PUSHJ P,SQRT
NSR: CAMLE AC,STOP+3 ;CHECK THE STOPS
JRST[ MOVE AC,STOP+3
JRST M3]
CAMGE AC,STOP+2
JRST[ MOVE AC,STOP+2
M3: SETOM FLAG
FADR AC,J+2
FSC AC,-1 ;HALF WAY BETWEEN STOP AND LAST JOINT ANGLE
JRST OK3]
OK3: MOVEM AC,J+2 ;JOINT THREE
;NOW COMPUTE THE BOOM OFFSET
MOVE AC,W2+1
FADR AC,W2
PUSH P,AC
PUSHJ P,SQRT
MOVM TAC,S2
FDVR TAC,AC
CAML TAC,[1.0]
MOVSI TAC,(1.0)
PUSH P,TAC
PUSHJ P,ASIN
MOVEM AC,JTEMP
;NOW COMPUTE TH FOR JOINT 1
PUSH P,W+1
PUSH P,W
PUSHJ P,ATAN2
IFN RH,<
FADR AC,JTEMP ;ADD J
>
IFN LH,<
FSBR AC,JTEMP
>
FMPR AC,RAD
MOVE TAC,AC
IFN RH,<
FSBR TAC,[90.0]
JUMPL TAC,ISOK
FSBR AC,[360.0]
>
IFN LH,<
FADR TAC,[90.0]
JUMPG TAC,ISOK
FADR AC,[360.0]
>
ISOK: CAMLE AC,STOP+1 ;AND CHECK THE STOPS FOR JOINT 1
JRST[ MOVE AC,STOP+1
JRST M1]
CAMGE AC,STOP
JRST[ MOVE AC,STOP
M1: SETOM FLAG
FADR AC,J
FSC AC,-1 ;HALF WAY BETWEEN STOP AND LAST JOINT ANGLE
JRST OK1]
OK1: MOVEM AC,J ;JOINT ONE
;JOINT 2-ACOS(W3/J3)
MOVN AC,W+2
FDVR AC,J+2
MOVEM AC,CO2
PUSH P,AC
PUSHJ P,ACOS
FMPR AC,RAD
MOVNM AC,J+1 ;JOINT TWO
;WE NOW NEED SOME SINES AND COSINES
PUSH P,J
PUSHJ P,SIND
MOVEM AC,SI1
PUSH P,J
PUSHJ P,COSD
MOVEM AC,CO1
PUSH P,J+1
PUSHJ P,SIND
MOVEM AC,SI2
;WE NOW COMPUTE THE UNIT VECTORS AT THE END OF LINK 3
MOVE AC,CO1
FMPR AC,CO2
MOVEM AC,Y
MOVE AC,SI1
FMPR AC,CO2
MOVEM AC,Y+1
MOVE AC,SI2
MOVNM AC,Y+2
FMPR AC,CO1
MOVEM AC,Z
MOVE AC,SI1
FMPR AC,SI2
MOVEM AC,Z+1
MOVE AC,CO2
MOVEM AC,Z+2
;CROSS(VT,Z,A);
PUSH P,[VT]
PUSH P,[Z]
PUSH P,[AV1]
PUSHJ P,CROSS
;CHECK FOR DEGENERATE CASE
PUSH P,[VT]
PUSHJ P,SECOND
CAMG AC,[0.000001]
JRST DEGEN ;JOINTS 4 AND 6 ARE DEGENERATE
PUSH P,AC
PUSHJ P,SQRT ;GET MAGNITUDE OF VT
MOVEI I,2
FVT: MOVE TAC,VT(I)
FDVR TAC,AC
MOVEM TAC,VT(I)
SOJGE I,FVT
VTF: ;VT NOW UNIT MAGNITUDE
;NOW JOINT 4
PUSH P,[VT]
PUSH P,[Y]
PUSH P,[Z]
PUSHJ P,ANGLE
MOVE TAC,AC
FSBR TAC,J+3
CAMLE TAC,[180.0]
JRST[ FSBR AC,[360.0]
JRST TN]
CAMGE TAC,[-180.0]
JRST[ FADR AC,[360.0]
JRST TN]
TN:
MOVE TAC,AC
FSBR TAC,J+3
CAMLE TAC,[90.0]
JRST[ FSBR AC,[180.0]
JRST RVTO]
CAMGE TAC,[-90.0]
JRST[ FADR AC,[180.0]
RVTO: MOVNS VT
MOVNS VT+1
MOVNS VT+2
JRST SOK4]
SOK4:
;NOW CHECK THE STOPS
CAMLE AC,STOP+5
JRST[ SKIPN PRINC
JRST[ MOVE AC,STOP+5
JRST M4]
FSBR AC,[180.0]
JRST RVT]
CAMGE AC,STOP+4
JRST[ SKIPN PRINC
JRST[ MOVE AC,STOP+4
M4: SETOM FLAG
FADR AC,J+3
FSC AC,-1
JRST OK4]
FADR AC,[180.0]
RVT: MOVNS VT
MOVNS VT+1
MOVNS VT+2
JRST OK4]
OK4: MOVEM AC,J+3 ;JOINT FOUR
;NOW JOINT 5
PUSH P,[AV1]
PUSH P,[Z]
PUSH P,[VT]
PUSHJ P,ANGLE
CAMLE AC,STOP+7
JRST[ MOVE AC,STOP+7
JRST M5]
CAMGE AC,STOP+6
JRST[ MOVE AC,STOP+6
M5: SETOM FLAG
FADR AC,J+4
FSC AC,-1 ;HALF WAY BETWEEN STOP AND LAST JOINT ANGLE
JRST OK5]
OK5: MOVEM AC,J+4
;JOINT 6 IS NOW SIMPLE
PUSH P,[OV1]
PUSH P,[VT]
PUSH P,[AV1]
PUSHJ P,ANGLE
TOL: MOVE TAC,AC
FSBR TAC,J+5
CAML TAC,[180.0]
JRST[ FSBR AC,[360.0]
JRST ST6]
CAMGE TAC,[-180.0]
JRST[ FADR AC,[360.0]
JRST ST6]
ST6: CAMLE AC,STOP+11
JRST[ SKIPN PRINC
JRST[ MOVE AC,STOP+11
JRST M6]
FSBR AC,[360.0]
JRST OK6]
CAMGE AC,STOP+10
JRST[ SKIPN PRINC
JRST[ MOVE AC,STOP+10
M6: SETOM FLAG
FADR AC,J+5
FSC AC,-1
JRST OK6]
FADR AC,[360.0]
JRST OK6]
OK6: MOVEM AC,J+5
HRRZ AC,-1(P)
HRRZI TAC,(AC)
HRLI AC,J
BLT AC,5(TAC)
MOVE AC,FLAG
SUB P,[3(3)]
JRST @3(P)
DEGEN: SETZM J+4 ;JOINT 5 IS ZERO
PUSH P,[OV1]
PUSH P,[Y]
PUSH P,[AV1]
PUSHJ P,ANGLE
FSBR AC,J+3 ;LEAVE JOINT 4 AS IS
JRST TOL ;AND PROCEED AS USUAL
PRINC: 0
FLAG: 0
JTEMP: 0
PV1: BLOCK 3
AV1: BLOCK 3
OV1: BLOCK 3
W: 0
SI1: 0
CO1: 0
W2: 0
SI2: 0
CO2: 0
VT: BLOCK 3
Y: BLOCK 3
Z: BLOCK 3
J: BLOCK 6
S2: S2V
S22: S22V
S6: S6V
TPI: 203622077325
STOP:
IFN BLUE,<
-90.0
190.0
6.2
34.2
-325.0
235.0
-105.0
105.0
-350.0
220.0
>
IFE BLUE,<
-185.0
60.0
6.5
27.5
-175.0
140.0
-101.0
101.0
-360.0
360.0
>
BEND
IFE WAVE,<
BEGIN UPDATE
;UPDATE(T,J)
;GIVEN JOINT ANGLES J COMPUTES THE TRANSFORMATION T
;TIME APPROX 6 M. SEC
A1←←2
A2←←3
A3←←4
R1←←5
R2←←6
R3←←7
I←←10
J←←11
K←←12
↑UPDATE:
HRL AC,-1(P) ;TH
HRRI AC,TH
BLT AC,TH+5
SETZM T ;SET T TO UNIT MATRIX
MOVE AC,[XWD T,T+1]
BLT AC,T+13
MOVSI AC,(1.0)
MOVEM AC,T
MOVEM AC,T+5
MOVEM AC,T+12
SETZM LINK
MATMUL: ;MATMUL T(K)←T(K-1)*A(K)
MOVE AC,[XWD T,TN]
BLT AC,TN+13 ;MOVE T INTO TN
MOVE K,LINK
CAIN K,2
JRST[ MOVE AC,SI3
MOVEM AC,SI
MOVE AC,CO3
MOVEM AC,CO
MOVE AC,TH+2
MOVEM AC,S+2
JRST IDSU]
PUSH P,TH(K)
PUSHJ P,SIND
MOVEM AC,SI
MOVE K,LINK
PUSH P,TH(K)
PUSHJ P,COSD
MOVEM AC,CO
IDSU: MOVE K,LINK
;FIRST ROW
MOVE A1,CO
MOVE A2,SI
SETZ A3,
MOVEI I,0
PUSHJ P,MUL
;SECOND ROW
MOVN A1,CA(K)
FMPR A1,SI
MOVE A2,CA(K)
FMPR A2,CO
MOVE A3,SA(K)
MOVEI I,1
PUSHJ P,MUL
;THIRD ROW
MOVE A1,SA(K)
FMPR A1,SI
MOVN A2,SA(K)
FMPR A2,CO
MOVE A3,CA(K)
MOVEI I,2
PUSHJ P,MUL
;FOURTH ROW
SETZ A1,
SETZ A2,
MOVE A3,S(K)
MOVEI I,3
PUSHJ P,MUL
MOVE AC,TN+3 ;OLD X
FADRM AC,T+3 ;NEW X
MOVE AC,TN+7 ;OLD Y
FADRM AC,T+7 ;NEW Y
MOVE AC,TN+13 ;OLD Z
FADRM AC,T+13 ;NEW Z
AOS K,LINK
CAIGE K,6
JRST MATMUL
HRRZ K,-2(P)
MOVEI AC,13(K)
HRLI K,T
BLT K,(AC)
HRRZ K,-2(P)
MOVE AC,SHOLDER
FADRM AC,3(K) ;X
MOVE AC,SHOLDER+1
FADRM AC,7(K) ;Y
MOVE AC,SHOLDER+2
FADRM AC,13(K) ;Z
SUB P,[3(3)]
JRST @3(P)
MUL: MOVEI J,2
L2: MOVE AC,PW(J)
BLT AC,R1+2
FMPR R1,A1
FMPR R2,A2
FMPR R3,A3
FADR R1,R2
FADR R1,R3
MOVEM R1,T(I)
ADDI I,4
SOJGE J,L2
POPJ P,
TH: BLOCK 6
PW: XWD TN+10,R1
XWD TN+4,R1
XWD TN,R1
LINK: 0
SI: 0
CO: 0
T: BLOCK 14
TN: BLOCK 14
SI3: -1.0
CO3: 0.0
S: 0.0
S2V
0.0
0.0
0.0
S6V
SA: -1.0
1.0
0.0
-1.0
1.0
0.0
CA: 0.0
0.0
1.0
0.0
0.0
1.0
BEND
BEGIN TRANSMULT
S←1
M←2
A1←3
A2←4
A3←5
A4←6
I←7
J←10
R←11
T←14
U←13
↑TIMES:HRL T,-1(P)
HRRI T,MLT
BLT T,MLT+13
MOVEI T,MLT
HRRZ U,-2(P)
HRRZ R,-3(P)
MOVEI I,3
L1: MOVEI J,3
MOVE A1,(U)
MOVE A2,1(U)
MOVE A3,2(U)
L2: MOVE S,A1
FMPR S,(T)
MOVE M,A2
FMPR M,4(T)
FADR S,M
MOVE M,A3
FMPR M,10(T)
FADR S,M
MOVEM S,(R)
AOJ R,
AOJ T,
SOJG J,L2
FMPR A1,(T)
FMPR A2,4(T)
FADR A1,A2
FMPR A3,10(T)
FADR A1,A3
FADR A1,3(U)
MOVEM A1,(R)
AOJ R,
SUBI T,3
ADDI U,4
SOJG I,L1
SUB P,[XWD 4,4]
JRST @4(P)
MLT: BLOCK 14
BEND
>
END